#ifndef VECTOR3_H
#define VECTOR3_H

#ifndef DOUBLE_PRECISION
#define DOUBLE_PRECISION 0.000001
#endif  /* DOUBLE_PRECISION */

#include <utils.h>
#include <math.h>

#include <stdlib.h>
#include <assert.h>

typedef struct vector3
{
  double data[ 3 ];
} vector3_t;


#define VECTOR3_X( v ) ( (v).data[ 0 ] )
#define VECTOR3_Y( v ) ( (v).data[ 1 ] )
#define VECTOR3_Z( v ) ( (v).data[ 2 ] )

vector3_t *
vector3_new( void );

void
vector3_free( vector3_t *v );

char *
vector3_str( const vector3_t v );

static inline vector3_t
vector3_init( double x, double y, double z )
{
  vector3_t r = { { x, y, z } };
  return r;
}

static inline void
vector3_set( vector3_t *v, double x, double y, double z )
{
  assert( v != NULL );
  VECTOR3_X( *v ) = x;
  VECTOR3_Y( *v ) = y;
  VECTOR3_Z( *v ) = z;
}

static inline double
vector3_abs( const vector3_t v )
{
  return sqrt( pow2( VECTOR3_X( v ) ) +
	       pow2( VECTOR3_Y( v ) ) +
	       pow2( VECTOR3_Z( v ) ) );
}

static inline vector3_t
vector3_normalize( const vector3_t v )
{
  double d = vector3_abs( v );

  assert( d != 0 );

  vector3_t r = vector3_init( VECTOR3_X( v ) / d,
			      VECTOR3_Y( v ) / d,
			      VECTOR3_Z( v ) / d );
  return r;
}

static inline vector3_t
vector3_negate( const vector3_t v )
{
  vector3_t r = vector3_init( - v.data[ 0 ], - v.data[ 1 ], - v.data[ 2 ] );
  return r;
}

static inline vector3_t
vector3_cross( const vector3_t a, const vector3_t b )
{
  vector3_t r = vector3_init(
    VECTOR3_Y( a ) * VECTOR3_Z( b ) - VECTOR3_Z( a ) * VECTOR3_Y( b ),
    VECTOR3_Z( a ) * VECTOR3_X( b ) - VECTOR3_X( a ) * VECTOR3_Z( b ),
    VECTOR3_X( a ) * VECTOR3_Y( b ) - VECTOR3_Y( a ) * VECTOR3_X( b )
    );

  return r;
}

static inline vector3_t
vector3_add( const vector3_t a, const vector3_t b )
{
  vector3_t r = vector3_init( VECTOR3_X( a ) + VECTOR3_X( b ),
			      VECTOR3_Y( a ) + VECTOR3_Y( b ),
			      VECTOR3_Z( a ) + VECTOR3_Z( b ) );
  return r;
}

static inline vector3_t
vector3_sub( const vector3_t a, const vector3_t b )
{
  vector3_t r = vector3_init( VECTOR3_X( a ) - VECTOR3_X( b ),
			      VECTOR3_Y( a ) - VECTOR3_Y( b ),
			      VECTOR3_Z( a ) - VECTOR3_Z( b ) );
  return r;
}

static inline double
vector3_dot( const vector3_t a, const vector3_t b )
{
  return \
    VECTOR3_X( a ) * VECTOR3_X( b ) +
    VECTOR3_Y( a ) * VECTOR3_Y( b ) +
    VECTOR3_Z( a ) * VECTOR3_Z( b );
}

static inline double
vector3_angle( const vector3_t a, const vector3_t b )
{
  double c = vector3_abs( a );
  double d = vector3_abs( b );

  assert( c != 0.0 );
  assert( d != 0.0 );

  return acos( vector3_dot( a, b ) / ( c * d ) );
}

static inline vector3_t
vector3_multiply_scalar( const vector3_t v, const double n )
{
  vector3_t r = vector3_init( VECTOR3_X( v ) * n,
			      VECTOR3_Y( v ) * n,
			      VECTOR3_Z( v ) * n );
  return r;
}

static inline int
vector3_is_equal( const vector3_t a, const vector3_t b )
{
  return \
    double_is_equal( VECTOR3_X( a ), VECTOR3_X( b ), DOUBLE_PRECISION ) &&
    double_is_equal( VECTOR3_Y( a ), VECTOR3_Y( b ), DOUBLE_PRECISION ) &&
    double_is_equal( VECTOR3_Z( a ), VECTOR3_Z( b ), DOUBLE_PRECISION );
}

static inline int
vector3_is_perpendicular( const vector3_t a, const vector3_t b )
{
  return double_is_equal( vector3_dot( a, b ), 0.0, DOUBLE_PRECISION );
}

static inline int
vector3_is_parallel( const vector3_t a, const vector3_t b )
{
  double angle = vector3_angle( a, b );

  return \
    double_is_equal( angle, 0.0, DOUBLE_PRECISION ) ||
    double_is_equal( angle, M_PI, DOUBLE_PRECISION );
}


#endif /* VECTOR3_H */
